#include "cpp_co_location_client/Airplane.hpp"
#include "cmath"

Eigen::MatrixX3d Airplane::getRotationMatrixYaw(double yaw) {
    Eigen::Matrix3d R;
    R << cos(yaw), -sin(yaw), 0,
         sin(yaw), cos(yaw), 0,
         0, 0, 1;
    return R;
}

Eigen::MatrixX3d Airplane::getRotationMatrixPitch(double pitch) {
    Eigen::Matrix3d R;
    R << cos(pitch), 0, sin(pitch),
         0, 1, 0,
         -sin(pitch), 0, cos(pitch);
    return R;
}

Eigen::MatrixX3d Airplane::getRotationMatrixRoll(double roll) {
    Eigen::Matrix3d R;
    R << 1, 0, 0,
         0, cos(roll), -sin(roll),
         0, sin(roll), cos(roll);
    return R;
}

Eigen::MatrixXd Airplane::computePseudoInverse(const Eigen::MatrixXd& matrix) {
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
    double tolerance = 1e-10;  // 设置一个阈值
    return svd.solve(Eigen::MatrixXd::Identity(matrix.cols(), matrix.cols()));
}

/**
 * @brief Airplane 初始化
 *
 * 根据给定的参数初始化 Airplane 对象。
 * 即设置该飞机的所有固定参数
 * @param 
 */
Airplane::Airplane(Eigen::Vector3d t_g,
    Rotation_Angles Pod_Rotation_Angles,Gimbal_Camera Gimbal_camera_parameter,
    Eigen::MatrixXd Ri)
{
    Pod_Translation_ = t_g;
    Pod_Rotation_Angles_ = Pod_Rotation_Angles;
    Pod_RotationMatrix_ = GenerateRotationMatrix(Pod_Rotation_Angles_);
    Gimbal_camera_parameter_ = Gimbal_camera_parameter;
    Ri_ = Ri;
}

bool Airplane::TargetIsLost(uint8_t TargetId)
{
    return TargetLostInfo_Map_[TargetId].TargetIsLost;
}

bool Airplane::DeleteTarget(uint8_t TargetId)
{
    
//  if(TargetLostInfo_Map_.count(TargetId))
    if (!(TargetLostInfo_Map_.find(0) == TargetLostInfo_Map_.end()))
    {
        Camera_Location_.erase(TargetId);
        CoRotationMatrix_.erase(TargetId);
        Measurements_[TargetId].clear();
        TargetLostInfo_Map_.erase(TargetId);
        return true;
    }
    else
    {
        //目标不存在，无需删除
        return true;
    } 
}

bool Airplane::UpdateTargetLostInfo_Map_(uint8_t TargetId, uint64_t Time_measure)
{
    // 如果目标map不存在,就新建一个
 //   if(!TargetLostInfo_Map_.count(TargetId))
    if ((TargetLostInfo_Map_.find(0) == TargetLostInfo_Map_.end()))
    {
        TargetLostInfo_Map_[TargetId];
    }
    // 更新丢失信息 TargetLostInfo_Map_
    if (TargetLostInfo_Map_[TargetId].TargetIsLost)
    {
        //目标之前为丢失状态
        TargetLostInfo_Map_[TargetId].TargetIsLost = false;
    }
    else{
        if((Time_measure - TargetLostInfo_Map_[TargetId].LastTimeFoundTarget)>TargetLostInfo_Map_[TargetId].TargetLostTimeMax)
        {
            // 目标出现的时间间隔上次量测过长，则判定为丢失状态。当Computer发现所有飞机均丢失目标，则重新调用S2来初始化ukf
            TargetLostInfo_Map_[TargetId].TargetIsLost = true;
        }
    }
    TargetLostInfo_Map_[TargetId].LastTimeFoundTarget = Time_measure;
    return true;
}

TargetLostInfo Airplane::GetTargetLostInfo(uint8_t TargetId)
{
    return TargetLostInfo_Map_[TargetId];
}

void Airplane::SetTargetLostInfoTargetLostTimeMax(uint8_t TargetId, uint64_t TargetLostTimeMax)
{
    TargetLostInfo_Map_[TargetId].TargetLostTimeMax = TargetLostTimeMax;
}

// 保存结果于Measurements_ 和 TargetIsAllLost = true时，执行S2，保存Camera_Location_ CoRotationMatrix_
bool Airplane::UpdateAirplaneState(Eigen::Vector3d t_n, Eigen::Vector3d t_c,
                         Rotation_Angles Gimbal_Rotation_Angles, Rotation_Angles Body_Rotation_Angles,
                         uint8_t TargetId, bool TargetIsAllLost,
                         Picture_Location PictureLocation, uint64_t Time_measure)
{
    Eigen::MatrixX3d Gimbal_RotationMatrix = GenerateRotationMatrix(Gimbal_Rotation_Angles);
    Eigen::MatrixX3d Body_RotationMatrix = GenerateRotationMatrix(Body_Rotation_Angles);
    Eigen::Vector3d t_g = Pod_Translation_;
    Eigen::MatrixXd MeasurementMatrix;
    if(TargetIsAllLost)
    {
        // 执行S2
        std::cout<< "S2 start!!"<<std::endl;
        Measurement_Pair measurement = GenerateMeasurement(Time_measure, PictureLocation, MeasurementMatrix, t_n.head(2), t_n(2));
        CoRotationMatrix_[TargetId] = GenerateCoRotationMatrix(Pod_RotationMatrix_, Gimbal_RotationMatrix, Body_RotationMatrix);
        Camera_Location_[TargetId] = GenerateCamera_Location_(t_g, t_c, t_n);
        Measurements_[TargetId].push_back(measurement);
    }
    else
    {
        // std::cout<<"Measurement size in airplane is:"<<int(Measurements_[TargetId].size())<<std::endl;
        // 执行S3
        MeasurementMatrix = GenerateMeasurementMatrix(t_g, t_c, t_n,
                                                      Pod_RotationMatrix_, Gimbal_RotationMatrix, Body_RotationMatrix,
                                                      Gimbal_camera_parameter_);
        Measurement_Pair measurement = GenerateMeasurement(Time_measure, PictureLocation, MeasurementMatrix, t_n.head(2),t_n(2));
        Measurements_[TargetId].push_back(measurement);
        // std::cout<<"Pod_RotationMatrix_:"<<Pod_RotationMatrix_<<std::endl;
        // std::cout<<"Gimbal_RotationMatrix:"<<Gimbal_RotationMatrix<<std::endl;
        // std::cout<<"Body_RotationMatrix:"<<Body_RotationMatrix<<std::endl;
        // std::cout<<"MeasurementMatrix:"<<MeasurementMatrix<<std::endl;
        // std::cout<<"Measurement size in airplane is:"<<int(Measurements_[TargetId].size())<<std::endl;
        // std::cout<<"measurement put in airplane!"<<std::endl;
    }
    is_initalized_ = true;
    return true;
}

bool Airplane::GetMeasurements_(std::vector<Measurement_Pair> &Measurements, uint8_t TargetId)
{
    if(!is_initalized_)
    {
        return false;
    }
    if(!Measurements_.count(TargetId))
    // if (std::find(Measurements_.begin(), Measurements_.end(), TargetId) == Measurements_.end()) 
    {   // 没有当前目标
        return false;
    }
    else
    {
        // std::cout<<"get measurements Measurement size in airplane is:"<<int(Measurements_[TargetId].size())<<std::endl;
        Measurements = Measurements_[TargetId];
        // auto clear all measurements
        Measurements_[TargetId].clear();
        // std::cout<<"Measurement size in airplane is:"<<int(Measurements_[TargetId].size())<<std::endl;
        return true;
    }
}

Eigen::MatrixX2d Airplane::GetRi_()
{
    return Ri_;
}

Eigen::Vector3d Airplane::GetPod_Translation_()
{
    // 固定参数，算法初始化时就有
    return Pod_Translation_;
}

Rotation_Angles Airplane::GetPod_Rotation_Angles_()
{
    return Pod_Rotation_Angles_;
}

Gimbal_Camera Airplane::GetGimbal_camera_parameter_()
{
    return Gimbal_camera_parameter_;
}

// 取出 //uint64_t time_now, uint64_t time_delta, 
bool Airplane::GetPicture_Location_(DDXTJC_Input &S2_Input,
                                    uint8_t TargetId)
{
    if(Measurements_[TargetId].empty())
    {
        return false;
    }
    uint64_t Measure_Time = Measurements_[TargetId].back().Time_measure;
    // // 判断量测时间是否满足要求
    // if( Measure_Time < (time_now - time_delta))
    // {
    //     return false;
    // }
    if(CoRotationMatrix_[TargetId].size()<1)
    {
        return false;
    }
    S2_Input.measure_time = Measure_Time;
    S2_Input.picture_loc = Measurements_[TargetId].back().PictureLocation;
    S2_Input.CoRotationMatrix = CoRotationMatrix_[TargetId];
    S2_Input.Camera_Location = Camera_Location_[TargetId];
    S2_Input.camera = Gimbal_camera_parameter_;
    // clear all measurements
    Measurements_[TargetId].clear();
    return true;
}

void Airplane::SetRi_(Eigen::MatrixX2d Ri)
{
    Ri_ = Ri;
}

void Airplane::SetPodTranslation_(Eigen::Vector3d t_g)
{
    Pod_Translation_ = t_g;
}

void Airplane::SetPod_Rotation_Angles_(Rotation_Angles Pod_Rotation_Angles)
{
    Pod_Rotation_Angles_ = Pod_Rotation_Angles;
    Pod_RotationMatrix_ = GenerateRotationMatrix(Pod_Rotation_Angles_);
}

void Airplane::SetGimbal_camera_parameter_(Gimbal_Camera Gimbal_camera_parameter)
{
    Gimbal_camera_parameter_ = Gimbal_camera_parameter;
}

void Airplane::SetNoiseCov_u(float noiseu)
{
    Ri_(0,0) = noiseu;
}
void Airplane::SetNoiseCov_v(float noisev)
{
    Ri_(1,1) = noisev;
}

void Airplane::SetPodTranslation_x(float x)
{
    Pod_Translation_(0) = x;
}
void Airplane::SetPodTranslation_y(float y)
{
    Pod_Translation_(1) = y;
}
void Airplane::SetPodTranslation_z(float z)
{
    Pod_Translation_(2) = z;
}

void Airplane::SetPod_Rotation_Angles_roll(float roll)
{
    Pod_Rotation_Angles_.roll = roll;
    Pod_RotationMatrix_ = GenerateRotationMatrix(Pod_Rotation_Angles_);
}
void Airplane::SetPod_Rotation_Angles_pitch(float pitch)
{
    Pod_Rotation_Angles_.pitch = pitch;
    Pod_RotationMatrix_ = GenerateRotationMatrix(Pod_Rotation_Angles_);
}
void Airplane::SetPod_Rotation_Angles_yaw(float yaw)
{
    Pod_Rotation_Angles_.yaw = yaw;
    Pod_RotationMatrix_ = GenerateRotationMatrix(Pod_Rotation_Angles_);
}

// void Airplane::SetGimbal_camera_parameter_f(float f)
// {
//     Gimbal_camera_parameter_.f = f;
// }
// void Airplane::SetGimbal_camera_parameter_d(float d)
// {
//     Gimbal_camera_parameter_.d = d;
// }


/**
 * 以下参数设定
 */
bool Airplane::GetCamera_Translation_(Eigen::Vector3d &Camera_Translation)
{
    if(Isinitalized())
    {
        Camera_Translation = Camera_Translation_;
        return true;
    }
    else
    {
        return false;
    }
}

// bool Airplane::GetBody_Translation_(Eigen::Vector3d &Body_Translation)
// {
//     if(Isinitalized())
//     {
//         Body_Translation = Body_Translation_;
//         return true;
//     }
//     else
//     {
//         return false;
//     }
// }

bool Airplane::GetGimbal_Rotation_Angles_(Rotation_Angles &Gimbal_Rotation_Angles)
{
    if(Isinitalized())
    {
        Gimbal_Rotation_Angles = Gimbal_Rotation_Angles_;
        return true;
    }
    else
    {
        return false;
    }
}

// bool Airplane::GetBody_Rotation_Angles_(Rotation_Angles &Body_Rotation_Angles)
// {
//     if(Isinitalized())
//     {
//         Body_Rotation_Angles = Body_Rotation_Angles_;
//         return true;
//     }
//     else
//     {
//         return false;
//     }
// }

Eigen::MatrixX3d Airplane::GenerateRotationMatrix(Rotation_Angles angles)
{
    // 
    Eigen::MatrixX3d MatrixYaw = getRotationMatrixYaw(angles.yaw);
    Eigen::MatrixX3d MatrixPitch = getRotationMatrixPitch(angles.pitch);
    Eigen::MatrixX3d MatrixRoll = getRotationMatrixRoll(angles.roll);
    Eigen::MatrixX3d RotationMatrix = MatrixRoll*MatrixPitch*MatrixYaw;
    return RotationMatrix;
}

// 生成旋转矩阵的集合 用于S2
Eigen::MatrixX3d Airplane::GenerateCoRotationMatrix(Eigen::MatrixX3d Pod_RotationMatrix, Eigen::MatrixX3d Gimbal_RotationMatrix, Eigen::MatrixX3d Body_RotationMatrix)
{
    // 飞机 吊舱 云台
    return Body_RotationMatrix * Pod_RotationMatrix * Gimbal_RotationMatrix;
}

// 生成相机光心在世界坐标系下的位置 用于S2
Eigen::Vector3d Airplane::GenerateCamera_Location_(Eigen::Vector3d t_g, Eigen::Vector3d t_c, Eigen::Vector3d t_n)
{
    return t_g + t_c + t_n;
}



// 生成观测矩阵模型H*z 3*4矩阵, 用于S3
Eigen::MatrixXd Airplane::GenerateMeasurementMatrix(Eigen::Vector3d t_g, Eigen::Vector3d t_c, Eigen::Vector3d t_n,
                                          Eigen::MatrixX3d Pod_RotationMatrix, Eigen::MatrixX3d Gimbal_RotationMatrix, Eigen::MatrixX3d Body_RotationMatrix,
                                          Gimbal_Camera Gimbal_camera_parameter)
{
    Eigen::MatrixXd Tg(4,4);
    Eigen::MatrixXd Tb(4,4);
    Eigen::MatrixXd Tn(4,4);
    Eigen::MatrixXd Tw(4,4);
    Eigen::MatrixXd zeros31(3,1);
    zeros31.fill(0.0);
    Eigen::MatrixXd zeros13(1,3);
    zeros13.fill(0.0);
    Eigen::MatrixXd Eye3 = Eigen::MatrixXd::Identity(3,3);
    Tg.fill(0.0);
    Tb.fill(0.0);
    Tn.fill(0.0);
    Tw.fill(0.0);
    // std::cout<<"t_g:"<<t_g<<std::endl;
    // std::cout<<"t_c:"<<t_c<<std::endl;
    // std::cout<<"t_n:"<<t_n<<std::endl;

    Tg << Gimbal_RotationMatrix, t_c,
            zeros13, 1;
    Tb << Pod_RotationMatrix, t_g,
            zeros13, 1;
    Tn << Body_RotationMatrix, zeros31,
            zeros13, 1;
    Tw << Eye3, t_n,
          zeros13, 1;
    // std::cout<<"Tw:"<<Tw<<std::endl;
    // // 求逆 适用于稀疏矩阵
    // Tg = computePseudoInverse(Tg);
    // Tb = computePseudoInverse(Tb);
    // Tn = computePseudoInverse(Tn);
    // Tw = computePseudoInverse(Tw);

    // 求逆 适用于稀疏矩阵
    Tg = Tg.completeOrthogonalDecomposition().pseudoInverse();
    Tb = Tb.completeOrthogonalDecomposition().pseudoInverse();
    Tn = Tn.completeOrthogonalDecomposition().pseudoInverse();
    Tw = Tw.completeOrthogonalDecomposition().pseudoInverse();

    // std::cout<<"Tg:"<<Tg<<std::endl;
    // std::cout<<"Tb:"<<Tb<<std::endl;
    // std::cout<<"Tn:"<<Tn<<std::endl;
    // std::cout<<"Tw_inv:"<<Tw<<std::endl;
    double fx = Gimbal_camera_parameter.fx;
    double fy = Gimbal_camera_parameter.fy;
    double cx = Gimbal_camera_parameter.cx;
    double cy = Gimbal_camera_parameter.cy;
    Eigen::MatrixXd F1(3,4);
    // std::cout<<"Gimbal_camera_parameter.f:"<<Gimbal_camera_parameter.f<<std::endl;
    // std::cout<<"Gimbal_camera_parameter.d:"<<Gimbal_camera_parameter.d<<std::endl;
    // std::cout<<"fd:"<<fd<<std::endl;
    F1.fill(0.0);// matrix p
    F1<<fx,0, cx,0,
            0,fy,cy,0,
            0,0,1,0;
    // 拼接
    Eigen::MatrixXd result;
    result = F1*Tg*Tb*Tn*Tw;
    return result;
}

Measurement_Pair Airplane::GenerateMeasurement(uint64_t Time_measure, Picture_Location PictureLocation, Eigen::MatrixXd MeasurementMatrix, Eigen::Vector2d airplane_xy, double z)
{
    Measurement_Pair Result;
    Result.Time_measure = Time_measure;
    Result.PictureLocation = PictureLocation;
    Result.MeasurementMatrix = MeasurementMatrix;
    Result.airplane_z = z;
    Result.airplane_xy = airplane_xy;
    
    return Result;
}

void Airplane::SetCoRotationMatrix(Eigen::MatrixXd CoRotationMatrix, uint8_t TargetId)
{
    CoRotationMatrix_[TargetId] = CoRotationMatrix;
}

void Airplane::SetCamera_Location_(Eigen::Vector3d Camera_Location,uint8_t TargetId)
{
    Camera_Location_[TargetId] = Camera_Location;
}
